%{
�������������:
- ���ε�ͼ���ϰ�
- ��ͬ�Ľ������
%}

function DrawSolution(results,model)

smooth=0.95;
N_algo = size(results.bestX,1); %�㷨����
color_mat = hsv(N_algo); % ���㷨����������ɫ
%% 3D�ӽ�
figure
PlotModel(model)
for n_algo=1:N_algo
    sol = Transforms(results.bestX(n_algo,:),model); % ת������
    x=sol(1,1:model.n);
    y=sol(1,model.n+1:2*model.n);
    z=sol(1,2*model.n+1:end);
    % ���
    xs=model.start(1);
    ys=model.start(2);
    zs=model.start(3);
    % �յ�
    xf=model.end(1);
    yf=model.end(2);
    zf=model.end(3);
    % ·��
    x_all = [xs x xf];
    y_all = [ys y yf];
    z_all = [zs z zf];
    
    N = size(x_all,2); %ʵ��·������
    % ·���߶�����ڵ���߶�
    for i = 1:N
        z_map = model.H(round(y_all(i)),round(x_all(i)));
        z_all(i) = z_all(i) + z_map;
    end
    
    %
    xyz = [x_all;y_all;z_all];
    [ndim,npts]=size(xyz);
    xyzp=zeros(size(xyz));
    for k=1:ndim
        xyzp(k,:)=ppval(csaps(1:npts,xyz(k,:),smooth),1:npts);
    end
    all_xyzp{n_algo}= xyzp; % �������е�ƽ�����·��
    %·�� 
    p1(n_algo) = plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),'color',color_mat(n_algo,:),'LineWidth',2);
end
% ���
p2 = plot3(x_all(1),y_all(1),z_all(1),'rp','MarkerSize',10,'MarkerFaceColor','r');
% �յ�
p3 = plot3(x_all(N),y_all(N),z_all(N),'ro','MarkerSize',7,'MarkerFaceColor','r');
view(-30,70)
legend([p1,p2,p3],results.alg_name{1,:},'Start','End','location','north','Orientation','horizon')
hold off;

%% Top�ӽ�
figure
surf(model.X,model.Y,model.H);
colormap summer;
shading flat;
material dull;
camlight left;
lighting gouraud;
xlabel('x [m]');
ylabel('y [m]');
zlabel('z [m]');
title('Top view')
hold on

% Threats as cylinders
threats = model.threats;
threat_num = size(threats,1);

for i = 1:threat_num
    threat = threats(i,:);
    threat_x = threat(1);
    threat_y = threat(2);
    threat_z = max(max(model.H))+1;  % ѡ��z��Ϊ��ߵķ�
    threat_radius = threat(4);
    
    %
    theta = linspace(0, 2 * pi, 2000);
    % ����ÿ���Ƕȵ�x��yλ��:
    xx = threat_radius * cos(theta) + threat_x;
    yy = threat_radius * sin(theta) + threat_y;
    % ��ҪΪÿ��(x,y)�Դ���һ��zֵ:752e29
    zz = zeros(1, numel(xx)) + threat_z;
    % ��һ��[160,94,166]./255
    plot3(threat_x, threat_y, threat_z, 'o', 'color',[150,95,160]./255 , 'MarkerSize', 3, 'MarkerFaceColor','#FFDAB9');
    % ��һ��
    plot3(xx, yy, zz, '-', 'color', '#FFDAB9', 'LineWidth', 1);
    
end
for n_algo=1:N_algo
    % ·��
    xyzp = all_xyzp{1, n_algo};
    p1(n_algo)=plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),'color',color_mat(n_algo,:),'LineWidth',2);
end
% ���
p2=plot3(x_all(1),y_all(1),z_all(1),'rp','MarkerSize',10,'MarkerFaceColor','r');
% �յ�
p3=plot3(x_all(N),y_all(N),z_all(N),'ro','MarkerSize',7,'MarkerFaceColor','r');
legend([p1,p2,p3],results.alg_name{1,:},'Start','End')

view(0,90)
%% ��������
figure
for n_algo=1:N_algo
    plot(results.cost_history(n_algo,:),'LineWidth',2);
    hold on
end
xlabel('Iteration');ylabel('Best Cost')
legend(results.alg_name  {1, :})
grid on;

end